#include "ros/ros.h"
#include "std_msgs/UInt8MultiArray.h"

#include <mbx.h>


#ifndef TASKNAME
        #define TASKNAME "ROSTSK" 
#endif
#ifndef MAILBOX_NAME_IN
        #define MAILBOX_NAME_IN "DRIMBX" //"ROS_IN"
#endif
#ifndef MAILBOX_NAME_OUT
        #define MAILBOX_NAME_OUT "DROMBX" //"ROSOUT"
#endif
#ifndef SAMPLETIME
        #define SAMPLETIME 500   /* must correspond to base time of Simulink/RTW target */
#endif				 /* sample rate is 500Hz for bipedRobin */

static RT_TASK *mytask;

MBX *mbxI, *mbxO;
int closingMbxFlagI = 0;
int closingMbxFlagO = 0;

int initializeRTMBX(void);
int terminateRTMBX(void);


/**
 * This node publishes data received via a mailbox from RTAI and puts data into the mailbox
 * subscribed by the noce
 */

void rtaiDataCallback(const std_msgs::UInt8MultiArray rtaiData) {
  int state=0;
  char dataIn[100000];
  ROS_INFO("Received MSG size to be sent: %d", rtaiData.data.size());
  copy(rtaiData.data.begin(), rtaiData.data.end(), (char*)&dataIn);
  if ((state = sendViaMbx(mbxI, (void*)&dataIn, rtaiData.data.size()))) {
    ROS_INFO("problem when sending via mbx %d", state);
  }
}

int main(int argc, char **argv)
{
  /* init ROS */
  ros::init(argc, argv, "ros_rtai_bridge_publisher");
  ros::NodeHandle n;
  ros::Publisher rtai_dataOut = n.advertise<std_msgs::UInt8MultiArray>("data_from_rtai", 1000, true);
  ros::Subscriber rtai_dataIn = n.subscribe<std_msgs::UInt8MultiArray>("data_to_rtai", 1000, rtaiDataCallback);
  ros::Rate loop_rate(SAMPLETIME); 

  /* init RTAI */
  ROS_INFO("Starting ROS RTAI bridge ...");
  if (initializeRTMBX() <= 0) {
    ROS_INFO("Error creating MBX Connection");
    ros::shutdown();
  }

  char dataOut[100000];
  ROS_INFO("Entering the loop ...");
  while (ros::ok()) { // SIGINT causes ros::ok() to be false 
    int count=0;
    count = receiveViaMbx(mbxO, (void*) &dataOut, sizeof(dataOut));

    if (count > 0) { //publish received data to ROS
      std_msgs::UInt8MultiArray msgOut;
      //TODO: fill out header of array!
      for (int i=0; i<count; i++) {
        msgOut.data.push_back(dataOut[i]);
      }
      rtai_dataOut.publish(msgOut);
    }

    ros::spinOnce();
    loop_rate.sleep();
  }

  ROS_INFO("Terminating ROS RTAI bridge ...");
  terminateRTMBX();

  return 0;
}

int initializeRTMBX(void) {
	rt_allow_nonroot_hrt();

	if (!(mytask = rt_task_init(nam2num(TASKNAME), 100, 0, 0))) {
		ROS_INFO("Error creating task TASK");
		return -1;
	}
	ROS_INFO("Initialized RTAI task...");

	mlockall(MCL_CURRENT|MCL_FUTURE);

	//opening MBX_IN
	if (openExistingMbx(&mbxI, MAILBOX_NAME_IN)) {
		ROS_INFO("Mailbox DRIMBX not opened yet, going to open mailbox ...");
		closingMbxFlagI = 1;
		if (openNewMbxBuf(&mbxI, MAILBOX_NAME_IN, 4804, 100)) {
			ROS_INFO("Could not open mailbox DRIMBX");
			return -1;
		} else {
			ROS_INFO("Opened mailbox IN...");
		}
	} else {
		ROS_INFO("Mailbox IN was already opened ...");
	}

	//opening MBX_OUT
	if (openExistingMbx(&mbxO, MAILBOX_NAME_OUT)) {
		ROS_INFO("Mailbox DROMBX not opened yet, going to open mailbox ...");
		closingMbxFlagO = 1;
		if (openNewMbxBuf(&mbxO, MAILBOX_NAME_OUT, 4804, 100)) {
			ROS_INFO("Could not open mailbox DROMBX");
			return -1;
		} else {
			ROS_INFO("Opened mailbox OUT...");
		}

	} else {
		ROS_INFO("Mailbox OUT was already opened ...");
	}

	return 1;
}

int terminateRTMBX(void) {
	if (closingMbxFlagI) {
		ROS_INFO("Closing MBX I ...");
		closeMbx(mbxI);
	}
	if (closingMbxFlagO) {
		ROS_INFO("Closing MBX O ...");
		closeMbx(mbxO);
	}
	munlockall();
	rt_task_delete(mytask);
	ROS_INFO("Deleted RTAI task...\n");
	return 1;
}
